/*
 * RiconoscimentoOstacolo.cpp
 */



/*
 * RiconoscimentoOstacolo.c
 */

#include "RiconoscimentoOstacolo.h"


RiconoscimentoOstacolo::RiconoscimentoOstacolo(){
}


Point RiconoscimentoOstacolo::riconoscimento_ostacolo(Mat rgbMat, Mat img_ostacolo_filt2){
	Mat pointsfOstacolo;
	vector<vector<Point> > contours_ostacolo;
	int ii;
	Rect maxRettangoloOstacolo;
	Point centroRettangoloOstacolo;
	bool ostacoloRiconosciuto=false;
	centroRettangoloOstacolo=Point(NULL,NULL);


	findContours(img_ostacolo_filt2, contours_ostacolo, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);
	vector<vector<Point2f> > contours_poligono_ostacolo( contours_ostacolo.size() );
	vector<Rect> rettangoli_ostacolo( contours_ostacolo.size());



	for(ii=0;ii<contours_ostacolo.size();ii++){
		Mat(contours_ostacolo[ii]).convertTo(pointsfOstacolo, CV_32F);
		approxPolyDP( pointsfOstacolo, contours_poligono_ostacolo[ii], 3, true );
		rettangoli_ostacolo[ii] = boundingRect( Mat(contours_poligono_ostacolo[ii]) );
	}

\
	for(ii=1;ii<contours_ostacolo.size();ii++){

		if(rettangoli_ostacolo[ii].area()>maxRettangoloOstacolo.area()&& rettangoli_ostacolo[ii].area()>MIN_AREA_OSTACOLO){
			ostacoloRiconosciuto=true;
			maxRettangoloOstacolo=rettangoli_ostacolo[ii];
		}
	}

	if(ostacoloRiconosciuto){
		rectangle( rgbMat,maxRettangoloOstacolo,Scalar(131,33,133),3);
		centroRettangoloOstacolo=Point(maxRettangoloOstacolo.x+maxRettangoloOstacolo.width/2,maxRettangoloOstacolo.y+maxRettangoloOstacolo.height/2);
		circle( rgbMat, centroRettangoloOstacolo, 3, Scalar(131,33,133), -1, 8, 0 );
	}

	return centroRettangoloOstacolo;

}

unsigned short RiconoscimentoOstacolo::calcoloProfonditaOstacolo(Mat rgbMat, Mat depthMat, Point centroRettangoloOstacolo){


	unsigned short profonditaOstacolo;
	char profonditaOstacoloString[20000];

	profonditaOstacolo= ((depthMat.at<unsigned short>(centroRettangoloOstacolo)));
	sprintf(profonditaOstacoloString,"%d mm",profonditaOstacolo);
	putText(rgbMat, profonditaOstacoloString, Point(centroRettangoloOstacolo.x,centroRettangoloOstacolo.y), 1, 2, Scalar(131,33,133),1,5,false);

	return profonditaOstacolo;

}


double RiconoscimentoOstacolo::calcoloAngoloOstacolo(Mat rgbMat,CvPoint centroRettangoloOstacolo){

	char angoloOstacoloString[10000];
	CvPoint cOstacolo;
	CvPoint ptOstacolo = cvPoint(rgbMat.cols/2,rgbMat.rows);
	cOstacolo=centroRettangoloOstacolo;

	//Applicazione del teorema dei seni
	int aOstacolo, bOstacolo;
	double dOstacolo, alfaOstacolo;
	bOstacolo = abs((ptOstacolo.y) - cOstacolo.y);
	aOstacolo = abs((cOstacolo.x) - ptOstacolo.x);
	dOstacolo = sqrt((aOstacolo*aOstacolo)+(bOstacolo*bOstacolo));
	alfaOstacolo = asin (aOstacolo/dOstacolo);

	//Assegnazione del segno all'angolo a seconda del quadrante in cui si trova
	if((cOstacolo.x) < rgbMat.cols/2)
		alfaOstacolo = (360 * (-1)*alfaOstacolo)/6.28;
	else
		alfaOstacolo = (360 * alfaOstacolo)/6.28;

	sprintf(angoloOstacoloString,"%.2f deg",alfaOstacolo);
	putText(rgbMat, angoloOstacoloString, Point(centroRettangoloOstacolo.x,centroRettangoloOstacolo.y+30), 1, 2, Scalar(131,33,133),1,5,false);

	return alfaOstacolo;


}





RiconoscimentoOstacolo::~RiconoscimentoOstacolo(){

}





